clear all; close all; clc

data = load('20110302-1.txt');

sT = 1/40;


phi = data(:,1);
theta = data(:,2);  
psi = data(:,3); 

phi_hat = data(:,4); 
theta_hat = data(:,5);
psi_hat = data(:,6);

phi_dot = data(:,7); 
theta_dot = data(:,8); 
psi_dot = data(:,9);

x = data(:,10);
y = data(:,11); 
z = data(:,12); 

x_hat = data(:,13);
y_hat = data(:,14); 
z_hat = data(:,15); 

x_dot = data(:,16); 
y_dot = data(:,17); 
z_dot = data(:,18); 

ppm1 = data(:,19);
ppm2 = data(:,20); 
ppm3 = data(:,21); 
ppm4 = data(:,22);

t = sT * [ 0 : length(data)-1 ];
%%
fg1 = figure(1)
set(fg1,'Color','White')
 hold on
plot(t, phi*180/pi, 'color', [1 0.5 0.5], 'linewidth', 2)
plot(t, theta*180/pi, 'color', [.4 1 0.3], 'linewidth', 2)
plot(t, psi*180/pi, 'color', [.4 0.2 1], 'linewidth', 2)
legend('phi', 'theta', 'psi')



%%
% ATT = figure(1);
% set(ATT,'Color','White')
% 
% PHI = subplot(3,1,1);
% set(PHI,'Box','on'), hold on
% plot(t,phi*180/pi), ylim([-20 20])
% % plot(t,phi_c*180/pi,'r')
% plot(t,phi_hat*180/pi,'r')
% % plot(t,phi_d_c*180/pi,'g','linewidth',2)
% % plot(t,phi_d*180/pi,'m','linewidth',2)
% 
% THETA  = subplot(3,1,2);
% set(THETA,'Box','on'), hold on
% plot(t,theta*180/pi), ylim([-20 20])
% % plot(t,theta_c*180/pi,'r')
% plot(t,theta_hat*180/pi,'r')
% % plot(t,theta_d_c*180/pi,'g','linewidth',2)
% % plot(t,theta_d*180/pi,'m','linewidth',2)
% 
% PSI  = subplot(3,1,3);
% set(PSI,'Box','on'), hold on
% plot(t,psi*180/pi), ylim([-20 20])
% % plot(t,psi_c*180/pi,'r')
% plot(t,psi_hat*180/pi,'r')
% % plot(t,psi_d*180/pi,'g','linewidth',2)
% 
% %%
% ATTDOT = figure(4);
% set(ATTDOT,'Color','White')
% 
% PHIDOT = subplot(3,1,1);
% set(PHIDOT,'Box','on'), hold on
% plot(t,phi_dot*180/pi)
% 
% THETADOT  = subplot(3,1,2);
% set(THETADOT,'Box','on'), hold on
% plot(t,theta_dot*180/pi)
% 
% PSIDOT  = subplot(3,1,3);
% set(PSIDOT,'Box','on'), hold on
% plot(t,psi_dot*180/pi)
% 
% %%
% POS = figure(2);
% set(POS,'Color','White')
% 
% XX = subplot(3,1,1);
% set(XX,'Box','on'), hold on
% plot(t,x), ylim([-1.5 1.5])
% plot(t,x_hat,'r')
% % plot(t,xd,'g','linewidth',2)
% 
% YY  = subplot(3,1,2);
% set(YY,'Box','on'), hold on
% plot(t,y), ylim([-1.5 1.5])
% plot(t,y_hat,'r')
% % plot(t,yd,'g','linewidth',2)
% 
% ZZ  = subplot(3,1,3);
% set(ZZ,'Box','on'), hold on
% plot(t,z), ylim([-0.5 1.2])
% plot(t,z_hat,'r')
% plot(t,zd,'g','linewidth',2)
% 
% %%
% Vel = figure(4);
% set(Vel,'Color','White')
% 
% XX = subplot(3,1,1);
% set(XX,'Box','on'), hold on
% plot(t,x_dot), ylim([-1.5 1.5]*3)
% % plot(t,x_hat,'r')
% % plot(t,xd,'g','linewidth',2)
% 
% YY  = subplot(3,1,2);
% set(YY,'Box','on'), hold on
% plot(t,y_dot), ylim([-1.5 1.5]*3)
% % plot(t,y_hat,'r')
% % plot(t,yd,'g','linewidth',2)
% 
% ZZ  = subplot(3,1,3);
% set(ZZ,'Box','on'), hold on
% plot(t,z_dot), ylim([-1.5 1.5]*3)
% % plot(t,z_hat,'r')
% % plot(t,zd,'g','linewidth',2)
% 
% %%
% INPUT = figure(3);
% set(INPUT,'Color','White')
% set(gca,'box','on')
% plot(t,ppm1,t,ppm2,t,ppm3,t,ppm4)
% 
% %%
% INT = figure(5);
% set(INT,'Color','White')
% set(gca,'box','on')
% 
% ATT_INT = subplot(2,1,1);
% plot(t,int_e_phi,t,int_e_theta,t,int_e_psi)
% legend('int e phi','int e theta','int e psi','location','northwest')
% 
% POS_INT = subplot(2,1,2);
% plot(t,int_e_x,t,int_e_y,t,int_e_z)
% legend('int e x','int e y','int e z','location','northwest')




